#include <iostream>
#include <fstream>
#include <opencv2/opencv.hpp>
#include <string>
#include <csignal>
#include <atomic>
#include <unistd.h>

#include "global.h"
#include "camera.h"
#include "control.h"
#include "Timer.h"
#include "serial.h"
#include "encoder.h"
#include "video.h"


std::atomic<bool> running(true);

void inputListener() {
    std::string input;
    while (true) {
        std::getline(std::cin, input); // 等待用户输入
        if (input == "s") {
            stop = true;
            std::cout << "标志位stop为true" << std::endl;
        }
        else if (input == "a") {
            turn_left = true;
            std::cout << "标志位 turn_left 为 true" << std::endl;
        }else if (input == "q"){
            obstacle_true = true;
            std::cout << "标志位 obstacle_true 为 true" << std::endl;
        }else if (input == "e"){
            obstacle_true = true;
            std::cout << "标志位 obstacle_true 为 true" << std::endl;
        }else if (input == "r") {  // 增加速度
            target_speed += 2;
            std::cout << "速度增加至: " << target_speed << std::endl;
        }
        else if (input == "t") {  // 减少速度
            target_speed -= 1;
            if (target_speed < 0) target_speed = 0;  // 确保速度不小于0
            std::cout << "速度减少至: " << target_speed << std::endl;
        }
    }
}


void signalHandler(int signal)
{
    running.store(false);
    ControlExit(); // 确保调用电机停止
}

int main(void)
{   
    std::signal(SIGINT, signalHandler);

    bool run;
    std::cout << "是否运动？(0 or 1):";
    std::cin >> run;

    // 启动输入监听线程
    std::thread inputThread(inputListener);
    inputThread.detach(); // 用于将线程从主线程​​分离​，让它独立运行，分离的线程在运行结束后会自动释放其资源

    double dest_fps = readDoubleFromFile(destfps_file); // 从配置文件中读取目标帧率，即想要达到的帧率

    int dest_frame_duration = CameraInit(0, dest_fps, 320, 240); // 调用CameraInit函数初始化摄像头设备，并返回每帧处理的理想时间间隔
    printf("%d\n", dest_frame_duration);

    if (dest_frame_duration != -1) // 即摄像头初始化成功
    {
        streamCaptureRunning = true;
        std::thread camworker = std::thread(streamCapture); // 摄像头流捕获线程
        std::cout << "Stream Capture Service started!\n";


        Timer MortorTimer(8, std::bind(ControlMain)); // 定时器线程, 每​​8毫秒​​执行一次ControlMain（电机控制逻辑）
        if(run == 1){
            ControlInit();
            std::cout << "Control Initialized!\n";
            MortorTimer.start();
            std::cout << "Control Service started!\n";
        }
        Timer CameraTimer(dest_frame_duration, std::bind(CameraHandler)); // 定时器线程, 每dest_frame_duration毫秒（由摄像头初始化决定）执行一次 CameraHandler（处理摄像头帧）
        CameraTimer.start();

        // 主循环，直到用户输入 Ctrl+C
        while (running.load())
        {   
            if (stop) {
                //printf("error:-11.0952\nerror:-11.0952\nerror:-11.2153\nerror:-11.6364\nerror:-14.4454\nerror:-14.4454\nerror:-0.68\nerror:-0.68\nerror:-0.68");
                std::cout << "********人行道前停车礼让行人***************************************************************人行道前停车礼让行人**********************************************************************人行道前停车礼让行人************************************************************人行道前停车礼让行人************************************************************************************人行道前停车礼让行人*******************************************人行道前停车礼让行人****************************************人行道前停车礼让行人*********************************************************************人行道前停车礼让行人*****************************************人行道前停车礼让行人**********************************************************人行道前停车礼让行人**********************************" << std::endl;
                target_speed = 0;
                sleep(3);
                stop = false; // 重置标志位
            }else{
                target_speed = readDoubleFromFile(speed_file);
            }

            mortor_kp = readDoubleFromFile(mortor_kp_file);
            mortor_ki = readDoubleFromFile(mortor_ki_file);
            mortor_kd = readDoubleFromFile(mortor_kd_file);

            kp = readDoubleFromFile(kp_file);
            ki = readDoubleFromFile(ki_file);
            kd = readDoubleFromFile(kd_file);
            // vofa_image(1, 160*120, 160, 120, Format_Grayscale8, (char*)IMG);

        }
        std::cout << "Stopping!\n";
        std::this_thread::sleep_for(std::chrono::milliseconds(500));

        CameraTimer.stop();
        std::cout << "Camera Timer stopped!\n";
        if(run == 1){
            MortorTimer.stop();
            std::cout << "Control Timer stopped!\n";

            ControlExit();
            std::cout << "Control Service stopped!\n";
        }

        streamCaptureRunning = false;
        if (camworker.joinable())
        {
            camworker.join();
        }

        cameraDeInit();
        std::cout << "Camera Service stopped!\n";
    }
    return 0;
}

